#include <time.h>
#include "CpuControlledVertibrateObject.h"
#include "include.h"
#include "GL/glut.h"

CpuControlledVertibrateObject::CpuControlledVertibrateObject(TextureManager * textureManager,
					 			  Vector2D position,
					  			  Vector2D * vertices,
					  			  int numVertices,
					  			  float mass,
				      			  bool isGrabbable,
				      			  char * skeletonFile)
:ControlledVertibrateObject(textureManager, position, vertices, numVertices, mass, isGrabbable, skeletonFile)
{
	char ** tags = (char**)malloc(sizeof(char*)*2);
	tags[0] = (char*)malloc(sizeof(char)*7);
	tags[1] = (char*)malloc(sizeof(char)*6);
	strcpy(tags[0],"wander");
	strcpy(tags[1],"stand");
	path = new NavigationEdgeList();
	navigationEdgeList = new NavigationEdgeList(new NavigationEdge( position + Vector2D(10,0), position - Vector2D(10,0), 
															walk, walk,tags,2));
	currentNavigationEdge = NULL;
	navigationEdgeList = NavigationEdgeList::BuildGraph(navigationEdgeList);
	currentStateMachine = NULL;
	currentGoal = NULL;
	lastGoal = NULL;
	CreateGoalList();	
}
CpuControlledVertibrateObject::CpuControlledVertibrateObject(TextureManager * textureManager,
					 			  Vector2D position,
					  			  Vector2D * vertices,
					  			  int numVertices,
					  			  float mass,
				      			  bool isGrabbable,
				      			  char * skeletonFile,
				      			  NavigationEdgeList * navigationEdgeList)
:ControlledVertibrateObject(textureManager, position, vertices, numVertices, mass, isGrabbable, skeletonFile)
{
	this->navigationEdgeList = navigationEdgeList;
	path = new NavigationEdgeList();
	currentNavigationEdge = NULL;
	this->navigationEdgeList = NavigationEdgeList::BuildGraph(navigationEdgeList);
	currentStateMachine = NULL;
	currentGoal = NULL;
	lastGoal = NULL;
	CreateGoalList();	
}
CpuControlledVertibrateObject::~CpuControlledVertibrateObject()
{
	if(path)
	{
		path->Clear();
		delete(path);
		path = NULL;
	}
	if(keyedGoalStateMachines)
	{
		delete(keyedGoalStateMachines);
		keyedGoalStateMachines = NULL;
	}
	if(navigationEdgeList)
	{	
		delete(navigationEdgeList);
		navigationEdgeList = NULL;
	}
}				      			  
void CpuControlledVertibrateObject::ControlledObjectDrawEntryPoint()
{
	printf("CpuControlledVertibrateObject::ControlledObjectDrawEntryPoint\n");
	glPushMatrix();
	glLoadIdentity();
	glDisable(GL_TEXTURE_2D);
	
	glColor3f(1,0,0);
	
	if(path)
	{
		path->RunFunction(&NavigationEdge::Draw);
	}
	
	glColor3f(1,1,1);
	if(navigationEdgeList)
		navigationEdgeList->RunFunction(&NavigationEdge::Draw);	
		
	
	glPopMatrix();
}
void CpuControlledVertibrateObject::ControlledObjectThinkEntryPoint(double elapsedTime)
{
	
	currentNavigationEdge = navigationEdgeList->GetCurrentEdge(position, boundingRadius, desiredNavigationEdge);
	if(currentNavigationEdge)
	{
		
		if(currentStateMachine)
		{
			//we have a current goal chain, how far are we through it right now?
			lastGoal = currentGoal;
			currentGoal = currentStateMachine->GetCurrentGoal(this);
			if(currentGoal)
			{
				if(currentGoal != lastGoal)
				{
					desiredNavigationEdge = currentGoal->FindSuitableLocation(this);
				}
				BuildPath();
				if(currentNavigationEdge == desiredNavigationEdge || desiredNavigationEdge == NULL)
				{
					//get to work on this goal
					currentGoal->behavior->Execute(this);
				}
				else
				{
					FollowPath();
				}
			}
			else
			{
				//we have met our goal and need a new one
				ChooseGoal();
			}
		}
		else
			ChooseGoal();
	}
	else
	{
		//We are off our edge graph. Panic!
		
	}
}
void CpuControlledVertibrateObject::CreateGoalList()
{
	//these are goals that all characters inheriting from this class will have.	
	//wander
	//stand
	
	GoalList * wanderGoalList = new GoalList(new Goal(new Behavior("walk", &CpuControlledVertibrateObject::DoWander),
										 &CpuControlledVertibrateObject::IsWanderDone, 
										 &CpuControlledVertibrateObject::FindWanderLocation));
	GoalList * standGoalList = new GoalList(new Goal(new Behavior("stand", &CpuControlledVertibrateObject::DoStand),
										 &CpuControlledVertibrateObject::IsStandDone, 
										 &CpuControlledVertibrateObject::FindStandLocation));
	keyedGoalStateMachines = new KeyedList<GoalList>("wander", wanderGoalList);
	keyedGoalStateMachines->Add("stand",standGoalList);
}
void CpuControlledVertibrateObject::ChooseGoal()
{
	//this one can only wander and stand, so choose randomly
	desiredNavigationEdge = NULL;
	if( (int)((rand()/(double)RAND_MAX)*10) < 8)
	{
		printf("Choosing wander\n");
		currentStateMachine = keyedGoalStateMachines->GetValue("wander");
	}
	else
	{
		printf("Choosing stand\n");
		currentStateMachine = keyedGoalStateMachines->GetValue("stand");
	}
}
void CpuControlledVertibrateObject::BuildPath()
{
	//find a path from the currentNavigationEdge to the desiredNavigationEdge
	//run some A*
	if(!desiredNavigationEdge)
		return;
	
	currentNavigationEdge->cameFrom = NULL;
	NavigationEdgeList * closedSet = NULL;//new NavigationEdgeList();
	NavigationEdgeList * openSet = new NavigationEdgeList(currentNavigationEdge);
	currentNavigationEdge->g = 0;
	currentNavigationEdge->h = currentNavigationEdge->GetMidpoint() | desiredNavigationEdge->GetMidpoint();
	currentNavigationEdge->f = currentNavigationEdge->h;

	while(!(openSet->IsEmpty()))
	{
		NavigationEdge * edge = openSet->PriorityPop();	
		if(edge == desiredNavigationEdge)
		{
			//now I need to reconstruct the path I took to get here
			
			path->Clear();
			while(edge)
			{
				
				path->Add(edge);
				edge = edge->cameFrom;
			}
			
			return;
		}
		if(closedSet)
			closedSet->Add(edge);
		else
			closedSet = new NavigationEdgeList(edge);
		NavigationEdgeList * neighbors = new NavigationEdgeList();
		
		NavigationEdgeList * rightEdges = edge->GetRightEdges();
		NavigationEdgeList * leftEdges = edge->GetLeftEdges();
		if(rightEdges)
		{
			neighbors->Merge(rightEdges);
		}
		if(leftEdges)
		{
			neighbors->Merge(leftEdges);
		}
			
		
		List<NavigationEdge> * search = neighbors->head;
		while(search)
		{
			
			if(search->payload)
			{
				
				if(!closedSet->Contains(search->payload))
				{
					
					float preG = edge->g + (edge->GetMidpoint() | search->payload->GetMidpoint());
					bool isBetter = false;
					
					if(!(openSet->Contains(search->payload)))
					{
						
						openSet->Add(search->payload);
						search->payload->h = (search->payload->GetMidpoint() | desiredNavigationEdge->GetMidpoint());
						isBetter = true;
					}
					else if(preG < search->payload->g)
					{
						isBetter = true;
					}
					if(isBetter)
					{
						
						search->payload->cameFrom = edge;
						search->payload->g = preG;
						search->payload->f = search->payload->g + search->payload->h;
					}
				}
			}
			
			search = search->next;	
		}
		
		neighbors->Clear();
		delete(neighbors);
		neighbors = NULL;
	}
	path->Clear();
	
	//failure
}
void CpuControlledVertibrateObject::FollowPath()
{
	printf("CpuControlledVertibrateObject::FollowPath\n");
	if(path->head)
	{
		if(path->head->payload == currentNavigationEdge)
		{
			path->Pop();	
		}
		NavigationEdge * target = path->head->payload;
		//I am on currentNavigationEdge and I am trying to get to target
		/*if((target->GetMidPoint().x < position.x)
		{
			printf("move left\n");
			MoveLeft();
		}
		else
		{
			printf("move right\n");
			MoveRight();
		}*/
		if((target->GetMidpoint() | currentNavigationEdge->GetRightEndPoint()) <
		   (target->GetMidpoint() | currentNavigationEdge->GetLeftEndPoint()) )
		{
			if(currentNavigationEdge->GetRightEndPoint().x < position.x)
			{
				MoveLeft();
			}
			else
				MoveRight();
				
			if((currentNavigationEdge->GetRightEndPoint().y > (position.y + boundingRadius))||
			   (currentNavigationEdge->GetLeftToRight() == jump))
			{
				Jump();
			}
			/*if(currentNavigationEdge->GetLeftToRight() == jump)
			{
				printf("jump1\n");
				Jump();
			}*/
		}
		else
		{
			if(currentNavigationEdge->GetLeftEndPoint().x < position.x)
			{
				MoveLeft();
			}
			else
				MoveRight();
				
			if((currentNavigationEdge->GetLeftEndPoint().y > (position.y + boundingRadius))||
			   (currentNavigationEdge->GetRightToLeft() == jump))
			{
				Jump();
			}
			/*if(currentNavigationEdge->GetRightToLeft() == jump)
			{
				printf("jump2\n");
				Jump();
			}*/
		}	   	
	}
	else
		printf("no path\n");
	
}
void CpuControlledVertibrateObject::MoveToEdge(NavigationEdge * edge)
{
	//find a path and follow it	
}
void CpuControlledVertibrateObject::DoWander()
{
	skeleton->SetAnimation("walk",0,0,1);
}
NavigationEdge * CpuControlledVertibrateObject::FindWanderLocation()
{
	
	int index = (int)((rand()/(double)RAND_MAX)*(navigationEdgeList->GetLength()-1));
	printf("wander index = %d\n", index);
	List<NavigationEdge> * search = navigationEdgeList->head;
	while(search)
	{
		if(index==0)
		{
			if(search->payload->IsTagged("wander"))
				return search->payload;
			else
				return FindWanderLocation();
		}
		index--;
		search = search->next;
	}
	return NULL;
}
bool CpuControlledVertibrateObject::IsWanderDone()
{
	currentNavigationEdge = navigationEdgeList->GetCurrentEdge(position, boundingRadius, desiredNavigationEdge);
	return currentNavigationEdge  && (currentNavigationEdge == desiredNavigationEdge);
}
void CpuControlledVertibrateObject::DoStand()
{
	skeleton->SetAnimation("stand",0,0,1);
}
NavigationEdge * CpuControlledVertibrateObject::FindStandLocation()
{
	
	return FindClosestTaggedLocation("stand");
}
NavigationEdge * CpuControlledVertibrateObject::FindClosestTaggedLocation(char * tag)
{
	
	float minDist = 9999;
	NavigationEdge * minEdge = NULL;
	List<NavigationEdge> * search = navigationEdgeList->head;
	while(search)
	{
		
		if(search->payload && search->payload->IsTagged(tag))
		{
			float dist = (position | search->payload->GetMidpoint());
			printf("checking dist = %f\n",dist);
			if(dist < minDist)
			{
				minDist = dist;
				minEdge = search->payload;
			}
		}
		search = search->next;
	}
	return minEdge;
}
bool CpuControlledVertibrateObject::IsStandDone()
{
	return ((int)((rand()/(double)RAND_MAX)*5000)) < 5;
}
void CpuControlledVertibrateObject::Jump()
{
	if(isOnGround)
	{
		isOnRightWall = false;
		isOnLeftWall = false;
		Vector2D up = Vector2D(0,1);//vertices[1] - vertices[2];
		up = !up * JUMP_FORCE * 0.25 *mass;
	
		AddForce(up);
	}
	else if(isOnRightWall)
	{
		isOnRightWall = false;
		isOnLeftWall = false;
		Vector2D left = vertices[0] - vertices[1];
		left = (!left + Vector2D(0,1)) * 0.5;
		left = left * WALLJUMP_FORCE * mass;
		AddForce(left);
	}
	else if(isOnLeftWall)
	{
		isOnRightWall = false;
		isOnLeftWall = false;
		Vector2D right = vertices[1] - vertices[0];
		right = (!right + Vector2D(0,1)) * 0.5;
		right = right * WALLJUMP_FORCE * mass;
		AddForce(right);
	}
	else
	{
	}
	isOnGround = false;
}

